#include "coordtrans.h"
#include "facedetect.h"
#include "cv.h"
#include <list>

#define CASCADE_NAME	"./haarcascade_frontalface_alt.xml"
#ifdef DEBUG
#include "motrecreader.h"
#endif

CoordinateTransformer::CoordinateTransformer() {
	_last_origin.first = -1;
	_last_origin.second = -1;
	_last_unit = -1;
	_cascade.load(CASCADE_NAME);
}

CoordinateTransformer::~CoordinateTransformer() {
}

int CoordinateTransformer::detect_face_intern (IplImage *img, CvPoint &center, int &rad) {
	cv::Mat frame, frameCopy;
    double scale = 1;
	frame = img;
	std::list<CvPoint> cent;
	std::list<int> rads;
	if( frame.empty() )
		return -1;
	if( img->origin == IPL_ORIGIN_TL )
		frame.copyTo( frameCopy );
	else
		flip( frame, frameCopy, 0 );

	detect_face(frameCopy, _cascade, scale, 1, cent, rads);
	std::list<int>::iterator rad_iter = rads.begin(); 
	for (std::list<CvPoint>::iterator iter = cent.begin(); 
			iter != cent.end(); ++iter, ++rad_iter) {
#ifdef DEBUG
		cvCircle(img, *iter, *rad_iter, CV_RGB(0,128,255), 3, 8, 0 );
		printf("center: (%d, %d); rad: %d\n", iter->x, iter->y, *rad_iter);
#endif
		center = *iter;
		rad = *rad_iter;
	}
	return 0;
}

int CoordinateTransformer::mark_face(IplImage *img) {
	CvPoint center;
	int rad;
	detect_face_intern(img, center, rad);
	cvCircle(img, center, rad, CV_RGB(0,0,255));
	return 0;
}

int CoordinateTransformer::coord_convert(
		Movement &mov,
		IplImage *img) {
	CvPoint origin;
	CvPoint face_center;
	int face_rad;
	int unit;

	/* Face Detection */
	if (mov.coord == MOVEMENT_COORD_FACE_CENT 
			|| mov.unit == MOVEMENT_COORD_UNIT_FACE_RAD
			|| _last_unit < 0 
			|| _last_origin.first < 0 || _last_origin.second < 0) {
#ifdef DEBUG
		printf("I need to do face detection now:\n");
		printf("last origin: (%d, %d); unit: %d\n",
				_last_origin.first, _last_origin.second, _last_unit);
		print_movement(mov);
#endif
		detect_face_intern(img, face_center, face_rad);
	}
	if (mov.coord == MOVEMENT_COORD_LAST_COORD) {
		if (_last_origin.first < 0 || _last_origin.second < 0) {
			origin = face_center;
		} else {
			origin.x = _last_origin.first;
			origin.y = _last_origin.second;
		}
	} else {
		origin = face_center;
	}
	if (mov.unit == MOVEMENT_COORD_UNIT_LAST) {
		if (_last_unit < 0) {
			unit = face_rad;
		} else {
			unit = _last_unit;
		}
	} else {
		unit = face_rad;
	}

	_last_origin.first = origin.x;
	_last_origin.second = origin.y;
	_last_unit = unit;

	printf("Current Origin: (%d, %d); Unit: %d\n",
			origin.x, origin.y, unit);

	mov.points_on_img.clear();
	for (std::list<std::pair<double, double> >::iterator iter = mov.points.begin();
			iter != mov.points.end(); iter++) {
		CvPoint new_point;
		/* Convert to new unit */
		new_point.x = iter->first * unit;
		new_point.y = iter->second * unit;

		/* Translate to new coordinate */
		new_point.x = new_point.x + origin.x;
		new_point.y = origin.y - new_point.y;

		/*
		printf("\tPoint: (%d, %d)\n", new_point.x, new_point.y);
		*/
		mov.points_on_img.push_back(new_point);
	}
	return 0;
}

int CoordinateTransformer::coord_convert(
		std::list<Movement> &mov,
		IplImage *img) {
	for (std::list<Movement>::iterator iter = mov.begin();
			iter != mov.end(); ++iter) {
		coord_convert(*iter, img);
	}
	return 0;
}
